#!/usr/bin/env python

import math
import rospy
from std_msgs.msg import Float64

def mover():
    # 发布每个关节的控制指令
    pub_j1 = rospy.Publisher('/arm/joint1_position_controller/command', Float64, queue_size=10)
    pub_j2 = rospy.Publisher('/arm/joint2_position_controller/command', Float64, queue_size=10)
    pub_j3 = rospy.Publisher('/arm/joint3_position_controller/command', Float64, queue_size=10)
    pub_j4 = rospy.Publisher('/arm/joint4_position_controller/command', Float64, queue_size=10)
    pub_j5 = rospy.Publisher('/arm/joint5_position_controller/command', Float64, queue_size=10)
    pub_j6 = rospy.Publisher('/arm/joint6_position_controller/command', Float64, queue_size=10)
    
    rospy.init_node('arm_mover')
    rate = rospy.Rate(10)  # 10Hz 发布频率
    start_time = 0

    # 等待时间初始化
    while not start_time:
        start_time = rospy.Time.now().to_sec()

    # 主循环
    while not rospy.is_shutdown():
        elapsed = rospy.Time.now().to_sec() - start_time
        # 每个关节周期性移动，使用正弦函数
        position = math.sin(2 * math.pi * 0.1 * elapsed) * (math.pi / 2)
        pub_j1.publish(position)
        pub_j2.publish(position)
        pub_j3.publish(position)
        pub_j4.publish(position)
        pub_j5.publish(position)
        pub_j6.publish(position)
        
        rate.sleep()

if __name__ == '__main__':
    try:
        mover()
    except rospy.ROSInterruptException:
        pass

